/**************************************************************************************************
* Title: SumQuaternion_16f.cpp
* Author: Gael Huber
* Description: Implements functions useful for 16-byte aligned quaternions
**************************************************************************************************/
#include "SumMath.h"

/**************************************************************************************************
* Compute a quaternion's axis and angle of rotation (expects unit quaternions)
**************************************************************************************************/
void QuaternionToAxisAngle_16f(const Quaternion_16f* pQ, Vector3_16f* pAxis, float* pAngle) {
	*pAngle = acosf(pQ->w);
	float s(1.0f / sinf(*pAngle));
	*pAngle *= 2.0f;
	pAxis->x = pQ->x * s;
	pAxis->y = pQ->y * s;
	pAxis->z = pQ->z * s;
}

/**************************************************************************************************
* Build a quaternion from a rotation matrix
**************************************************************************************************/
Quaternion_16f* QuaternionRotationMatrix_16f(Quaternion_16f* pOut, const Matrix_16f* pM) {
	// Find the largest diagonal
	if(pM->_11 > pM->_22 && pM->_11 > pM->_33) {
		pOut->x = pM->_11 - pM->_22 - pM->_33 + 1.0f;
		pOut->y = pM->_12 + pM->_21;
		pOut->z = pM->_13 + pM->_31;
		pOut->w = pM->_32 - pM->_23;
	} else if(pM->_22 > pM->_11 && pM->_22 > pM->_33) {
		pOut->x = pM->_22 - pM->_11 - pM->_33 + 1.0f;
		pOut->y = pM->_12 + pM->_21;
		pOut->z = pM->_23 + pM->_32;
		pOut->w = pM->_13 - pM->_31;
	} else {
		pOut->x = pM->_33 - pM->_11 - pM->_22 + 1.0f;
		pOut->y = pM->_13 + pM->_31;
		pOut->z = pM->_32 + pM->_23;
		pOut->w = pM->_21 - pM->_12;
	}
	return pOut;
}

/**************************************************************************************************
* Rotation about an arbitrary axis
**************************************************************************************************/
Quaternion_16f* QuaternionRotationAxis_16f(Quaternion_16f* pOut, const Vector3_16f* pV, float angle) {
	float s(sinf(angle * 0.5f));
	pOut->x = s * pV->x;
	pOut->y = s * pV->y;
	pOut->z = s * pV->z;
	pOut->w = cosf(angle * 0.5f);
	return pOut;
}

/**************************************************************************************************
* Yaw around the Y axis, a pitch around the X axis, and a roll around the Z axis
**************************************************************************************************/
Quaternion_16f* QuaternionRotationYawPitchRoll_16f(Quaternion_16f* pOut, float yaw, float pitch, float roll) {
	Quaternion_16f q1, q2, q3;

	// Calculate yaw and pitch
	QuaternionRotationAxis_16f(&q1, &Vector3_16f(0.0f, 1.0f, 0.0f), yaw);
	QuaternionRotationAxis_16f(&q2, &Vector3_16f(1.0f, 0.0f, 1.0f), pitch);
	QuaternionMultiply_16f(&q3, &q1, &q2);
	
	// Calculate roll
	QuaternionRotationAxis_16f(&q1, &Vector3_16f(0.0f, 0.0f, 1.0f), roll);
	QuaternionMultiply_16f(pOut, &q3, &q1);

	// Return the result
	return pOut;
}

/**************************************************************************************************
* Quaternion multiplication (Q2 * Q1)
**************************************************************************************************/
Quaternion_16f* QuaternionMultiply_16f(Quaternion_16f* pOut, const Quaternion_16f* pQ1,
	const Quaternion_16f* pQ2) {
	pOut->v = _mm_add_ps(
		_mm_mul_ps(_mm_load1_ps(&pQ1->x), _mm_set_ps(-pQ2->x, -pQ2->y, pQ2->z, pQ2->w)),
		_mm_mul_ps(_mm_load1_ps(&pQ1->y), _mm_set_ps(-pQ2->y, pQ2->x, pQ2->w, -pQ2->z)));
	pOut->v = _mm_add_ps(
		pOut->v,
		_mm_mul_ps(_mm_load1_ps(&pQ1->z), _mm_set_ps(-pQ2->z, pQ2->w, -pQ2->x, pQ2->y)));
	pOut->v = _mm_add_ps(
		pOut->v,
		_mm_mul_ps(_mm_load1_ps(&pQ1->w), pQ2->v));
	return pOut;
}

/**************************************************************************************************
* Multiply a quaternion by a Vector3
**************************************************************************************************/
Vector3_16f* QuaternionMultiply_16f(Vector3_16f* pOut, const Quaternion_16f* pQ, const Vector3_16f* pV) {
	Vector3_16f qVec(pQ->x, pQ->y, pQ->z);
	Vector3_16f uv, uuv;
	Vec3Cross_16f(&uv, &qVec, pV);
	Vec3Cross_16f(&uuv, &qVec, &uv);
	Vec3Scale_16f(&uv, &uv, 2.0f * pQ->w);
	Vec3Scale_16f(&uuv, &uuv, 2.0f);
	Vec3Add_16f(pOut, pV, &uv);
	Vec3Add_16f(pOut, pOut, &uuv);
	return pOut;
}

/**************************************************************************************************
* Normalize a quaternion
**************************************************************************************************/
Quaternion_16f* QuaternionNormalize_16f(Quaternion_16f* pOut, const Quaternion_16f* pQ) {
	float l(1.0f / QuaternionLength_16f(pQ));
	pOut->v = _mm_mul_ps(pQ->v, _mm_load1_ps(&l));
	return pOut;
}

/**************************************************************************************************
* Returns the inverse of the quaternion (conjugate and re-norm)
**************************************************************************************************/
Quaternion_16f* QuaternionInverse_16f(Quaternion_16f* pOut, const Quaternion_16f* pQ) {
	QuaternionConjugate_16f(pOut, pQ);
	QuaternionNormalize_16f(pOut, pOut);
	return pOut;
}

/**************************************************************************************************
* Takes the natural log of the quaternion. Expects unit quaternions. 
* If q = (cos(theta), sin(theta) * v); ln(q) = (0, theta * v)
**************************************************************************************************/
Quaternion_16f* QuaternionLn_16f(Quaternion_16f* pOut, const Quaternion_16f* pQ) {
	float theta(acosf(pQ->w));
	float s(theta / sinf(theta));

	// Set new quaternion
	pOut->x = pQ->x * s;
	pOut->y = pQ->y * s;
	pOut->z = pQ->z * s;
	pOut->w = 0.0f;
	return pOut;
}

/**************************************************************************************************
* Determines the exponentiation of a quaternion. Expects pure quaternions. w == 0; w is ignored in
* calculation. If q = (0, theta * v); exp(q) = (cos(theta), sin(theta) * v)
**************************************************************************************************/
Quaternion_16f* QuaternionExp_16f(Quaternion_16f* pOut, const Quaternion_16f* pQ) {
	float length(QuaternionLength_16f(pQ));

	if(length > 0.0f) {
		pOut->w = cosf(length);
		float s(sinf(length) / length);
		pOut->x = pQ->x * s;
		pOut->y = pQ->y * s;
		pOut->z = pQ->z * s;
	} else {
		pOut->x = pOut->y = pOut->z = pOut->w = 0.0f;
	}
	return pOut;
}

/**************************************************************************************************
* Spherical linear interpolation between Q1 (t == 0) and Q2 (t == 1). Expects unit quaternions
**************************************************************************************************/
Quaternion_16f* QuaternionSlerp_16f(Quaternion_16f* pOut, const Quaternion_16f* pQ1, 
	const Quaternion_16f* pQ2, float t) {
	// Compute the difference between Q1 and Q2 (dQ)
	Quaternion_16f q1, q2;
	QuaternionInverse_16f(&q1, pQ1);
	QuaternionMultiply_16f(&q2, &q1, pQ2);

	// Take a fraction t of dQ through exponentiation
	float theta(acosf(q2.w));
	float newTheta(theta * t);
	__m128 mult(_mm_set1_ps(sinf(newTheta) / sinf(theta)));
	q2.v = _mm_mul_ps(q2.v, mult);
	q2.w = cosf(newTheta);

	// Modify the original by the fraction
	QuaternionMultiply_16f(pOut, pQ1, &q2);

	return pOut;
}

/**************************************************************************************************
* Spherical quadrangle interpolation (Slerp(Slerp(Q1, C, t), Slerp(A, B, t), 2t(1-t))
**************************************************************************************************/
Quaternion_16f* QuaternionSquad_16f(Quaternion_16f* pOut, const Quaternion_16f* pQ1,
	const Quaternion_16f* pA, const Quaternion_16f* pB, const Quaternion_16f* pC, float t) {
	Quaternion_16f s1, s2;
	QuaternionSlerp_16f(&s1, pQ1, pC, t);
	QuaternionSlerp_16f(&s2, pA, pB, t);
	QuaternionSlerp_16f(pOut, &s1, &s2, 2.0f * t * (1.0f - t));
	return pOut;
}

/**************************************************************************************************
* Setup control points for spherical quadrangle interpolation from Q1 to Q2. The contorl points are
* chosen in such a way to ensure the continuity of tangents with adjacent segments.
**************************************************************************************************/
void QuaternionSquadSetup_16f(Quaternion_16f* pAOut, Quaternion_16f* pBOut, Quaternion_16f* pCOut,
	const Quaternion_16f* pQ0, const Quaternion_16f* pQ1, const Quaternion_16f* pQ2,
	const Quaternion_16f* pQ3) {
	// Load q0, q1, q2, q3
	Quaternion_16f tq1, tq2;
	QuaternionAdd_16f(&tq1, pQ0, pQ1);
	QuaternionSub_16f(&tq2, pQ0, pQ1);
	Quaternion_16f q0(QuaternionLength_16f(&tq1) < QuaternionLength_16f(&tq2) ? -*pQ0 : *pQ0);
	Quaternion_16f q1(*pQ1);

	QuaternionAdd_16f(&tq1, pQ1, pQ2);
	QuaternionSub_16f(&tq2, pQ1, pQ2);
	Quaternion_16f q2(QuaternionLength_16f(&tq1) < QuaternionLength_16f(&tq2) ? -*pQ2 : *pQ2);

	QuaternionAdd_16f(&tq1, pQ2, pQ3);
	QuaternionSub_16f(&tq2, pQ2, pQ3);
	Quaternion_16f q3(QuaternionLength_16f(&tq1) < QuaternionLength_16f(&tq2) ? -*pQ3 : *pQ3);

	Quaternion_16f qE, qLN1, qLN2, t;

	// Calculate A
	QuaternionExp_16f(&qE, pQ1);
	QuaternionLn_16f(&qLN1, &(qE*q2));
	QuaternionLn_16f(&qLN2, &(qE*q1));
	QuaternionAdd_16f(&tq1, &qLN1, &qLN2);
	QuaternionScale_16f(&tq1, &tq1, -0.25f);
	QuaternionExp_16f(&t, &tq1);
	QuaternionMultiply_16f(pAOut, &q1, &t);

	// Calculate B
	QuaternionExp_16f(&qE, pQ2);
	QuaternionLn_16f(&qLN1, &(qE*q3));
	QuaternionLn_16f(&qLN2, &(qE*q1));
	QuaternionAdd_16f(&tq1, &qLN1, &qLN2);
	QuaternionScale_16f(&tq1, &tq1, -0.25f);
	QuaternionExp_16f(&t, &tq1);
	QuaternionMultiply_16f(pBOut, &q2, &t);

	// Calculate C
	*pCOut = q2;
}

/**************************************************************************************************
* Barycentric interpolation. Slerp(Slerp(Q1, Q2, f+g), Slerp(Q1, Q3, f+g), g/(f+g))
**************************************************************************************************/
Quaternion_16f* QuaternionBarycentric_16f(Quaternion_16f* pOut, const Quaternion_16f* pQ1, const Quaternion_16f* pQ2, 
	const Quaternion_16f* pQ3, float f, float g) {
	// Internal slerps
	Quaternion_16f q1, q2;
	QuaternionSlerp_16f(&q1, pQ1, pQ2, f + g);
	QuaternionSlerp_16f(&q2, pQ1, pQ3, f + g);
	
	// External slerp
	QuaternionSlerp_16f(pOut, &q1, &q2, g / (f + g));
	
	return pOut;
}